/*
 * Servo NETMF Driver
 *	Coded by Chris Seto August 2010
 *	<chris@chrisseto.com> 
 *	
 * Use this code for whatveer you want. Modify it, redistribute it, I don't care.
 * I do ask that you please keep this header intact, however.
 * If you modfy the driver, please include your contribution below:
 * 
 * Chris Seto: Inital release (1.0)
 * Chris Seto: Netduino port (1.0 -> Netduino branch)
 * Chris Seto: bool pin state fix (1.1 -> Netduino branch)
 * 
 * */

using System;
using Microsoft.SPOT.Hardware;
using SecretLabs.NETMF.Hardware;

namespace Servo.Netduino
{
    public class Servo : IDisposable
    {
        /// <summary>
        /// PWM handle
        /// </summary>
        private PWM servo;

        /// <summary>
        /// Set servo inversion
        /// </summary>
        public bool inverted = false;

        /// <summary>
        /// Servo model settings
        /// </summary>
        readonly Servos servoModel;

        /// <summary>
        /// Create the PWM pin, set it low and configure timings
        /// </summary>
        /// <param name="pin"></param>
        public Servo(Cpu.Pin pin, Servos servoModel)
        {
            // Init the PWM pin
            servo = new PWM((Cpu.Pin)pin);

            servo.SetDutyCycle(0);

            this.servoModel = servoModel;
        }

        public void Dispose()
        {
            disengage();
            servo.Dispose();
        }

        /// <summary>
        /// Disengage the servo. 
        /// The servo motor will stop trying to maintain an angle
        /// </summary>
        public void disengage()
        {
            // See what the Netduino team say about this... 
            servo.SetDutyCycle(0);
        }

        /// <summary>
        /// Set the servo degree
        /// </summary>
        public double Degree
        {
            set
            {
                /// Range checks
                if (value > servoModel.angleMax)
                    value = servoModel.angleMax;

                if (value < servoModel.angleMin)
                    value = servoModel.angleMin;

                // Are we inverted?
                if (inverted)
                    value = servoModel.angleMax - value;

                // Set the pulse
                servo.SetPulse(servoModel.period, (uint)map((long)value, servoModel.angleMin, servoModel.angleMax, servoModel.range[0], servoModel.range[1]));
            }

            get 
            {
                throw new NotImplementedException("TODO");
            }
        }

        /// <summary>
        /// Set de servo to center value
        /// </summary>
        public void Center()
        {
            this.Degree = this.servoModel.CenterValue;
        }

        /// <summary>
        /// Return the max angle for these servo model
        /// </summary>
        public int MaxAngle 
        {
            get
            { return this.servoModel.angleMax; }
        }

        /// <summary>
        /// Return the min angle for these servo model
        /// </summary>
        public int MinAngle
        {
            get
            {
                return this.servoModel.angleMin;
            }
        }

        /// <summary>
        /// Used internally to map a value of one scale to another
        /// </summary>
        /// <param name="x"></param>
        /// <param name="in_min"></param>
        /// <param name="in_max"></param>
        /// <param name="out_min"></param>
        /// <param name="out_max"></param>
        /// <returns></returns>
        private long map(long x, long in_min, long in_max, long out_min, long out_max)
        {
            return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
        }
    }
}
